
#include <string.h>
#include <stdio.h>

#include<windows.h>

#include "PolhemusFrame.h"


// to complete PDI_MODATA_XXX defined in pdimdat.h
#define PDI_MODATA_HEADER	-1

// using namespace std ;
// #define ESC	0x1b


int PolhemusFrame::initialize(CPDIdev &pdiDev){

	CPDImdat mDat;
	unsigned long stationMap;
	/* 
	   count number of enabled sensors and establish mapping between 
	   sensor number and station number:
	   - stationNb is the port number where the sensor is physically 
	     installed and corresponds to the value returned in the output 
	     frame header
	  - sensorNB is used to index data strctures
	*/

	// get active station map
	if( !pdiDev.GetStationMap (stationMap)) { 
		strncpy(errorMsg,pdiDev.GetLastResultStr(),POL_ERROR_MSG_SIZE); 
		return FALSE;
	}
	printf("Active StationMap: %#x\r\n",stationMap );


	// count all activated sensors in all stations
	numSensors=0;
	for(unsigned short station=1;station<=POL_MAX_STATIONS;station++) {
		if(stationMap & 0x0001) {
			sensorNb[station]=numSensors;
			stationNb[numSensors]=station;
			numSensors++;
		}
		stationMap=stationMap>>1 ; // shift to the right
	}

	// Make a description of the output frame for the Decode functions by
	// initializing the following variables
	//		* numItems		number of items
	//		* items[]		item types
	//		* frameSize		frame size in bytes

	numItems=0;
	frameSize=0;
	for(int sensor=0;sensor<numSensors;sensor++){
		// header
		items[numItems]= PDI_MODATA_HEADER;
		frameSize+=8;		// header size(in bytes)
		numItems++;
		// get station output list for the sensor
		pdiDev.GetSDataList(stationNb[sensor], mDat);
		for(int i=0;i<mDat.NumItems();i++) {
			// item type
			items[numItems]=mDat.ItemAt(i);
			// number of bytes per item
			switch(items[numItems]) {
			case PDI_ODATA_SPACE:
				frameSize+=1; break;
			case PDI_MODATA_CRLF: 
				frameSize+=2; break;
			case PDI_MODATA_POS: 
			case PDI_MODATA_POS_EP:
			case PDI_MODATA_ORI: 
			case PDI_MODATA_ORI_EP: 
				frameSize+=12; break;
			case PDI_MODATA_DIRCOS: 
				frameSize+=36; break;
			case PDI_MODATA_QTRN: 
				frameSize+=16; break;
			case PDI_MODATA_TIMESTAMP: 
			case PDI_MODATA_FRAMECOUNT: 
			case PDI_MODATA_STYLUS: 
			case PDI_MODATA_DISTLEV: 
			case PDI_MODATA_EXTSYNC: 
//			case PDI_MODATA_CALFRAME: 
				frameSize+=4; break;
			default:
				return FALSE;
			}
			numItems++;
		} // numItems per sensor
	} // sensor

printf("numItems %d frameSize %d \n",numItems,frameSize);

	return TRUE;
}

int PolhemusFrame::getFrameSize(void){
	return frameSize;
}

int PolhemusFrame::decodeBinaryFrame(const unsigned char *frame){
	unsigned short tag;
	unsigned char station, error;
	int sensor; 
//	unsigned long fc, extsync;

	int frameIndex=0;
	// decode a frame
	for (int i=0; i<numItems; i++) {
		// decode output data list
        switch(items[i]){
		case PDI_MODATA_HEADER:
			//
			tag= *((unsigned short*)&frame[0]);
// printf("tag %x\n",tag);
			if(tag==0x0000) {
				sprintf_s(errorMsg,POL_ERROR_MSG_SIZE,"DecodeBinaryFrame - Bad tag (%x)",tag);
				return FALSE;
			}
			// station/sensor number (1 byte)
			station = (unsigned char)frame[2];
			sensor = sensorNb[station];
			// error code (BYTE)
			error = frame[4];
// printf("station %x sensor %d error %x\n",station,sensor,error);
			if(error!=0x00 && error!=0x20) {
				sprintf_s(errorMsg,POL_ERROR_MSG_SIZE,"DecodeBinaryFrame - Error code (%x)",error);
				return FALSE;
			}
			frameIndex += 8;
			break;
        case PDI_MODATA_POS:// 3xfloats (= 12 bytes
	        position[sensor][0] = *((float *)&frame[frameIndex]);
			position[sensor][1] = *((float *)&frame[frameIndex+4]);
	        position[sensor][2] = *((float *)&frame[frameIndex+8]);
		    frameIndex += 12;
// printf("position %f %f %f\n",position[sensor][0],position[sensor][1],position[sensor][2]);
		    break;
        case PDI_MODATA_ORI:			// 12 bytes
			orientation[sensor][0] = *((float *)&frame[frameIndex]);
		    orientation[sensor][1] = *((float *)&frame[frameIndex+4]);
            orientation[sensor][2] = *((float *)&frame[frameIndex+8]);
			frameIndex += 12;
// printf("orientation %f %f %f\n", orientation[sensor][0], orientation[sensor][1], orientation[sensor][2]);
            break;
		case PDI_MODATA_FRAMECOUNT:		// 4 bytes
			frameCount[sensor] = *((unsigned long*)&frame[frameIndex]);
			frameIndex += 4;
// printf("fc %lu\n",frameCount[sensor]);
			break;
		case PDI_MODATA_EXTSYNC: // 4 bytes
			extSync[sensor] = *((unsigned long*)&frame[frameIndex]);
			frameIndex += 4;
			break;
        default:
			return FALSE;
		    break;
		}
	} /* item */
	return TRUE;
}

int PolhemusFrame::getPosition(int sensor, float *val){
	val[0]=position[sensor][0];
	val[1]=position[sensor][1];
	val[2]=position[sensor][2];
	return TRUE;
}

int PolhemusFrame::getOrientation(int sensor, float *val){
	val[0]=orientation[sensor][0];
	val[1]=orientation[sensor][1];
	val[2]=orientation[sensor][2];
	return TRUE;
}

int PolhemusFrame::getFrameCount(int sensor, unsigned long &val){
	val = frameCount[sensor];
	return TRUE;
}

int PolhemusFrame::getExtSync(int sensor, unsigned long &val){
	val = extSync[sensor];
	return TRUE;
}